/***************************************************************************
 创建者: 华磊
 开始时间: 2018.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2017.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "motioninstructionlogic.h"
#include "GeneralDefine.h"
#include "programengine.h"
#include "messagelog.h"
#include "filteringalgorithm.h"
#include "gcodeparse.h"

#define D_FETCH_WAIT_COUNT 30
#define D_FETCH_SLEEP 50000
#define D_TRACK_MIN_DISTANCE 0.3
//#define D_FILTER_COUNT 5

MotionInstructionLogic::MotionInstructionLogic(QString configPathIn,int robotIdIn, ProgramSet* programSetIn,
                                               RobotIo* ioControllerIn, ProgramEngine* programEngineIn,
                                               McControllerThread *programMcControllerIn,
                                               QVector<PointPro> *getPointProListIn,
                                               QVector<PlRegStruct> *regPlListIn)
    {
        programSet=programSetIn;
        robotId=robotIdIn;
        ioController=ioControllerIn;
        programEngine=programEngineIn;
        programMcController=programMcControllerIn;
        getPointProList=getPointProListIn;
        regPlList=regPlListIn;
        configPath=configPathIn;
        lastAddedLineId=-1;
        RobotCommonParameter coordinateConfigOut;
        programMcController->mcControllerInterpreter->getRobotCommonPara(coordinateConfigOut);
        armTpType=coordinateConfigOut.armTpType;
        maxLineCatshDepth_carvingTp=coordinateConfigOut.maxLineCatshDepth_carvingTp;
        filterX=new FilteringAlgorithm;
        filterY=new FilteringAlgorithm;
        filterZ=new FilteringAlgorithm;
        gcodeParse=new GcodeParse;

        if(1!=initialConfigFile(configPath))
        {

            addMsg( ENUM_MSG_ERROR,  "MotionInstructionLogic", "LaserCameraAdapter",  3316,  robotId);

        }

}

E_PROGRAM_LOOP_RESULT MotionInstructionLogic::bufferMove(int programNum, int startLine, double velIn)
{
    E_BUFFER_MOVE_TYPE type=(*programSet)[programNum][startLine].bufferMoveIns.type;
    QVector<PointPro > keyPointsOut;
    QVector<PointPro > refOri;
    if(E_BUFFER_MOVE_ORI==type)
    {
        int p_num=(*programSet)[programNum][startLine].bufferMoveIns.oriPointIndex_1-1;
        if( p_num<getPointProList->size() && p_num >= 0)
        {
            refOri.append( (*getPointProList)[ p_num ]);
        }
        else
        {
            qDebug()<<"test common point,无此点位数据："<< p_num;
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3007, robotId ,p_num+1);
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        p_num=(*programSet)[programNum][startLine].bufferMoveIns.oriPointIndex_2-1;
        if( p_num<getPointProList->size() && p_num >= 0)
        {
            refOri.append( (*getPointProList)[ p_num ]);
        }
        else
        {
            qDebug()<<"test common point,无此点位数据："<< p_num;
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3007, robotId ,p_num+1);
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        p_num=(*programSet)[programNum][startLine].bufferMoveIns.oriPointIndex_3-1;
        if( p_num<getPointProList->size() && p_num >= 0)
        {
            refOri.append( (*getPointProList)[ p_num ]);
        }
        else
        {
            qDebug()<<"test common point,无此点位数据："<< p_num;
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3007, robotId ,p_num+1);
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }

    }
    if(1!=programEngine->getLaserScanTraj(refOri,keyPointsOut))
    {
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }
    if(0==keyPointsOut.size())
    {
        qDebug()<<"error,,,0==keyPointsOut.size()";
        programEngine->cmdError(programNum);
        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3007, robotId );
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }

    int lineId=startLine;

    MoveParameter tmpMovePara;
    QVector <MoveParameter> tmpMotionParaList;
    tmpMovePara.moveType = ENUM_COMMAND_LINE;
    tmpMovePara.pointType = ENUM_NORMAL_POINT;
    tmpMovePara.velocityUnit = E_RATE_MM_SEC;
    tmpMovePara.velocity = velIn/100.0;
    tmpMovePara.evFlag = 0;
    tmpMovePara.evVelocity = 0;
    tmpMovePara.wjnt = 0;
    tmpMovePara.pathFlag = 0;
    tmpMovePara.incFlag = 0;
    tmpMovePara.cntRatio = 50;
    tmpMovePara.acc = -1;


    for(int i=0;i<keyPointsOut.size();i++)
    {
        tmpMovePara.targetPoint=keyPointsOut[i];
        tmpMovePara.lineNumber = lineId;
        tmpMotionParaList.append(tmpMovePara);
        lineId++;

    }

    int tmpaddKey= programMcController->mcControllerInterpreter->transCommandQueueToCarvingModule
                                                                    (tmpMotionParaList);
    if(1==tmpaddKey)
    {
        lastAddedLineId=tmpMotionParaList.back().lineNumber;
        programEngine->setMotionInstructionAdded(tmpMotionParaList.back().lineNumber);//   最后加入的行号
    }
    else
    {
        qDebug()<<"add line error,line "<<startLine;
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }


    //等待行号
    E_PROGRAM_LOOP_RESULT tmpRsult=waitMotionAccomplish(lineId-1);
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=tmpRsult)
    {
        if(ENUM_COMMAND_RESET == programEngine->getEngineStatue())
        {
            qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();
            //恢复行号
            programEngine->setRunningNumber_robotArmControl(startLine);
           return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
        }
        else if(ENUM_COMMAND_CEASE == programEngine->getEngineStatue())
        {
            if(programEngine->getQuickStopFlag())
            {//报错后，轨迹被清空．重新运行该条命令
                if(1!=isMotionInstructionFinished(lineId))//一定要加行有没有完成判断
                {
                    //reset() 使下次在运行该行时，才能重新添加轨迹。
                    //恢复行号
                    programEngine->setRunningNumber_robotArmControl(startLine);
                    return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
                }

            }
        }
        return tmpRsult;
    }
    //恢复行号
    programEngine->setRunningNumber_robotArmControl(startLine);
    programEngine->calculateNextLineToRun(programNum);
    return programEngine->isTheLastInstruction(programNum);
}

int MotionInstructionLogic::addCircle(int idIn, std::vector<double> posIn, std::vector<double> centerIn,
                                      std::vector<double> normalIn, double vel, int turnsIn, int velTypeIn)
{
    qDebug()<<"MotionInstructionLogic::addCircle";
    int tmpKey=0;
    if(0==armTpType)
    {

    }
    else
    {
        tmpKey=programMcController->mcControllerInterpreter->addCircle(idIn,posIn,centerIn,normalIn,vel,turnsIn,velTypeIn);
    }
    return tmpKey;
}

E_PROGRAM_LOOP_RESULT MotionInstructionLogic::addProgramMoveCommand_hmi(vector<MoveParameter> &tmpCommandVectorIn)
{
    qDebug()<<"MotionInstructionLogic::addProgramMoveCommand_hmi";
    if( tmpCommandVectorIn.size() )
    {
        QVector<MoveParameter> tmpMotionParaList=QVector<MoveParameter>::fromStdVector(tmpCommandVectorIn);
        programEngine->setMotionInstructionAdded(tmpCommandVectorIn.size());
        if(0==armTpType)
        {
            programMcController->mcControllerInterpreter->addTrajectoryCommand(tmpMotionParaList);
        }
        else
        {
//            programMcController->mcControllerInterpreter->transCommandQueueToCarvingModule(tmpCom);
            int tmpKey=0;
            int tmpMoveType=tmpMotionParaList[0].moveType;
            if(1==tmpMotionParaList.size() &&
                    (ENUM_COMMAND_JOINT_VARIABLE==tmpMoveType||ENUM_COMMAND_SINGLE_AXIS_TRAP==tmpMoveType
                     || ENUM_COMMAND_SINGLE_AXIS_SPLINE==tmpMoveType||ENUM_COMMAND_JOINT_SPLINE==tmpMoveType))
            {
//                if(startLine!=tmpMotionParaList[0].lineNumber)
//                {
//                    addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3309, robotId ,tmpMotionParaList[0].lineNumber+1);
//                    qDebug()<<"error3309a,";
//                    return E_PROGRAM_LOOP_RESULT_ERROR;
//                }

                tmpKey= programMcController->mcControllerInterpreter->addTrajectoryCommand(tmpMotionParaList);

            }
            else
            {
                //判断是否掺杂关节
                for(int i=0;i<tmpMotionParaList.size();i++)
                {
                    tmpMoveType=tmpMotionParaList[i].moveType;
                    if((ENUM_COMMAND_JOINT_VARIABLE==tmpMoveType||ENUM_COMMAND_SINGLE_AXIS_TRAP==tmpMoveType
                             || ENUM_COMMAND_SINGLE_AXIS_SPLINE==tmpMoveType||ENUM_COMMAND_JOINT_SPLINE==tmpMoveType))
                    {
                        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3309, robotId ,tmpMotionParaList[i].lineNumber+1);
                        qDebug()<<"error3309b,";
                        return E_PROGRAM_LOOP_RESULT_ERROR;
                    }
                }
                tmpKey= programMcController->mcControllerInterpreter->transCommandQueueToCarvingModule(tmpMotionParaList);

            }

            if(1==tmpKey)
            {
                lastAddedLineId=tmpMotionParaList.back().lineNumber;
                programEngine->setMotionInstructionAdded(tmpMotionParaList.back().lineNumber);//   最后加入的行号
            }
            else
            {
                qDebug()<<"add line error,lastAddedLineId "<<lastAddedLineId;
            }
            return (E_PROGRAM_LOOP_RESULT)tmpKey;
        }

    }
    else
    {
        programEngine->setMotionInstructionAdded(0);
    }

    return E_PROGRAM_LOOP_RESULT_NORMAL;
}

int MotionInstructionLogic::addMonitorDi_fleet(bool isOr, QVector<int> diIndex, QVector<int> diValue)
{
    diMuTex.lock();
    fleetMonitor_isOr=isOr;
    fleetMonitor_diIndex=diIndex;
    fleetMonitor_diValue=diValue;
    diMuTex.unlock();
    return 1;
}

E_PROGRAM_LOOP_RESULT MotionInstructionLogic::moveRobotArm_run(const QVector<int> &lineList,
                                                               const QVector<RobotArmMoveParameter> &pathListParaIn, int isLastPointMove)
{
    if(0>=pathListParaIn.size())
    {
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }


    E_PROGRAM_LOOP_RESULT resultKey=addProgramMoveCommand_fleet(lineList,pathListParaIn);
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=resultKey)
    {
        return resultKey;
    }

    resultKey=waitMotionAccomplish_fleet(lineList);//如果出错会一直卡在这里。这时候需要停止或清除旧轨迹，才可以退出。
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=resultKey)
    {
        return resultKey;
    }


    return E_PROGRAM_LOOP_RESULT_NORMAL;
}

E_PROGRAM_LOOP_RESULT MotionInstructionLogic::startAxisMove(int programNum, int startLine, const AxisMoveInstruction &motionInstruction)
{
    qDebug()<<"startAxisMove startLine"<<startLine;
    switch(motionInstruction.positionReference)
    {
    case E_POSITION_REFERENCE_INC:
    case E_POSITION_REFERENCE_ABSOLUTE:
    {
        programMcController->mcControllerInterpreter->setCoupleControlEnable(motionInstruction.axisId-1,false);
        //添加轨迹指令
        if(1!=processAxisMoveInstruction( startLine,motionInstruction))
        {
            qDebug()<<"MotionInstructionLogic3 startMotion error,,,";
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }

        E_PROGRAM_LOOP_RESULT tmpRsult=waitMotionAccomplish(startLine);
        if(E_PROGRAM_LOOP_RESULT_NORMAL!=tmpRsult)
        {
            return tmpRsult;
        }

        break;
    }
    case E_POSITION_REFERENCE_FOLLOW:
    case E_POSITION_REFERENCE_MAP:
    {
        if(1!=programMcController->mcControllerInterpreter->setCoupleControlEnable
                                                            (motionInstruction.axisId-1,false))
        {
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3303, robotId, motionInstruction.axisId-1);
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        if(1!=processAxisMoveInstruction( startLine,motionInstruction))
        {
            qDebug()<<"MotionInstructionLogic3 startMotion error,,,";
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }

        E_PROGRAM_LOOP_RESULT tmpRsult=waitMotionAccomplish(startLine);
        if(E_PROGRAM_LOOP_RESULT_NORMAL!=tmpRsult)
        {
            return tmpRsult;
        }
        //设置耦合偏差.不用设置,集成到setCoupleControlEnable
//        programMcController->mcControllerInterpreter->resetCoupleOffset();
//        qDebug()<<"----------programMcController->mcControllerInterpreter->resetCoupleOffset();";
//        programMcController->mcControllerInterpreter->setCoupleControlOffset(motionInstruction.axisId,
//                                        motionInstruction.targetPosition);错误的
        //使耦合生效
        programMcController->mcControllerInterpreter->setCoupleControlEnable(motionInstruction.axisId-1,true);
        break;
    }
    default:
    {
        qDebug()<<"startAxisMove error,,,unsupport positionReference"<<motionInstruction.positionReference;
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }
    }

    programEngine->calculateNextLineToRun(programNum);

    E_PROGRAM_LOOP_RESULT endFlag = E_PROGRAM_LOOP_RESULT_NORMAL;
    endFlag = programEngine->isTheLastInstruction(programNum);
    return endFlag;

}

E_PROGRAM_LOOP_RESULT MotionInstructionLogic::startMotion(int programNum, int startLine)
{
    qDebug()<<"startMotion startLine"<<startLine;
    programEngine->setMotionStage(E_PROGRAM_MOTION_INIT);
    programEngine->updateProgramSetRegister();
    E_PROGRAM_LOOP_RESULT endFlag = E_PROGRAM_LOOP_RESULT_NORMAL;

    if(programEngine->getLaserCameraTrackStatus())
    {
        return laserCameraTrackLine(programNum,startLine);
    }

    if(0==armTpType)
    {
        if(0>processMotionInstruction( programNum ,startLine))
        {
            qDebug()<<"MotionInstructionLogic2 processMotionInstruction startMotion error,,,";
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
    }
    else
    {
        if(0>processMotionInstruction_carvingTp( programNum ,startLine))
        {
            qDebug()<<"MotionInstructionLogic2 processMotionInstruction_carvingTp startMotion error,,,";
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
    }



    //监视条件
    E_PROGRAM_LOOP_RESULT tmpRsult;
    int tmpDiIndex=(*programSet)[programNum][startLine].motion.diValue-1;
    switch((*programSet)[programNum][startLine].motion.motionDiType)
    {
    case MOTION_DI_NONE:
        tmpRsult=waitMotionAccomplish(startLine);
        break;
    case MOTION_DI_ON:
        tmpRsult=waitMotionAccomplish(startLine,true,tmpDiIndex,true);
        break;
    case MOTION_DI_OFF:
        tmpRsult=waitMotionAccomplish(startLine,true,tmpDiIndex,false);
        break;
    }

    //完成判断，没有则阻塞等待

    if(E_PROGRAM_LOOP_RESULT_NORMAL!=tmpRsult)
    {
        qDebug()<<"start motion return ..."<<tmpRsult;
        if(E_PROGRAM_LOOP_RESULT_JUMP==tmpRsult)
        {
            QString jumpLabel=(*programSet)[programNum][startLine].motion.lblValue;
            return programEngine->startJump(programNum,jumpLabel);
        }
        return tmpRsult;
    }



    programEngine->calculateNextLineToRun(programNum);
    endFlag = programEngine->isTheLastInstruction(programNum);
    return endFlag;
}

E_PROGRAM_LOOP_RESULT MotionInstructionLogic::startMotion_Gcode(int programNum, int startLine)
{
    qDebug()<<"startMotion_Gcode startLine"<<startLine;
    programEngine->setMotionStage(E_PROGRAM_MOTION_INIT);
//    programEngine->updateProgramSetRegister();
    E_PROGRAM_LOOP_RESULT endFlag = E_PROGRAM_LOOP_RESULT_NORMAL;

//    if(programEngine->getLaserCameraTrackStatus())
//    {
//        return laserCameraTrackLine(programNum,startLine);
//    }

    if(0>processMotionInstruction_gcode( programNum ,startLine))
    {
        qDebug()<<"MotionInstructionLogic2 processMotionInstruction_gcode startMotion error,,,";
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }


    E_PROGRAM_LOOP_RESULT tmpRsult=waitMotionAccomplish(startLine);


    //完成判断，没有则阻塞等待
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=tmpRsult)
    {
        return tmpRsult;
    }


    programEngine->calculateNextLineToRun(programNum);
    endFlag = programEngine->isTheLastInstruction(programNum);
    return endFlag;
}

int MotionInstructionLogic::clearAllMsg()
{
    lastAddedLineId=programMcController->mcControllerInterpreter->getRunningNumber()-1;
    return 1;
}




int MotionInstructionLogic::isMotionInstructionFinished(int programNum,int waitLine)
{
    //采用阻塞工作方式，等待单条运动指令移动完。

    //floor函数。 如： floor(2.5) = 2     ,  floor(-2.5) = -3
    //ceil函数。   如：  ceil(2.5) = 3       ,   ceil(-2.5) = -2     floor()是向负无穷大舍入，

//    int isFinished=-1;

//    //程序行号取整
//    int intProgramLine= waitLine;//0.9-> 0;    0.999999=1.0
//    //算法行号
//    int intMotionLine= programMcController->mcControllerInterpreter->getRunningNumber();//0.9-> 0
//    //行号整数差
//    float intLineDifference= programMcController->mcControllerInterpreter->getRunningNumber()- intProgramLine;//-0.5
//    //行号小数差
//    float floatLineDifference=programMcController->mcControllerInterpreter->getRunningNumber() - intMotionLine;//0.5

//    if( intMotionLine != intProgramLine && -1!=intMotionLine && intMotionLine!=lastMotionInstructionLine)
//    {
//        //算法比较快的情况,快一行以上
//        //追赶算法. 不能直接等过去, 否则可能会跳过中间的非运动指令
//        //qDebug() << "===intLineDifference=-===1111=="<<intLineDifference<<"===runningNumber==="<<programMcController->mcControllerInterpreter->runningLineNumber
//        //<<"***pCount***"<<pCount<<"----------(*programSet)[programNum].pc---------"<<(*programSet)[programNum].pc<<"\n\n";
//        (*programSet)[programNum].runningLineInt++;
//        programEngine->setRunningLineFloat((*programSet)[programNum].runningLineInt+0.02);//(防止丢失精度)

//        if(  programEngine->getRunModel()<ENUM_CONTROL_AUTO && programEngine->getMoveType()==ENUM_MOVE_ORDER_STEP  )
//        {
//            qDebug()<<"startMove======18.2===========算法比较快,单步模式暂停";
//            programEngine->set_mamual_run_flag(false) ;
//        }
//        isFinished=1;
////        lastMotionInstructionLine=intProgramLine;
//    }
//    else if( intMotionLine==intProgramLine )
//    {
//        if(  floatLineDifference > 0.4 && floatLineDifference < 0.6 )
//        {
//            // 正在运动 0.5
//            //return endFlag;
//        }
//        else if( floatLineDifference > 0.8 && floatLineDifference < 0.99 )
//        {
//            //算法运行完了 0.90
//            //qDebug() << "****pc** 算法运行完了一行**"<<(*programSet)[programNum].pc<< pCount ;

//           (*programSet)[programNum].runningLineInt++;
//           programEngine->setRunningLineFloat( (*programSet)[programNum].runningLineInt+0.02);//(防止丢失精度)

//            if(  programEngine->getRunModel()<ENUM_CONTROL_AUTO && programEngine->getMoveType()==ENUM_MOVE_ORDER_STEP  )
//            {
//                programEngine->set_mamual_run_flag(false);

//                qDebug()<<"======18===========暂停";

//            }
//            isFinished=1;
////            lastMotionInstructionLine=intProgramLine;
//        }
//    }
//    return isFinished;

}

//enum MC_COMMAND
//{
//    ENUM_COMMAND_JOINT_VARIABLE = 0,
//    ENUM_COMMAND_LINE = 1,
//    ENUM_COMMAND_CIRCLE = 2,
//    ENUM_COMMAND_JOINT_CONTINIOUS = 3,
//    ENUM_COMMAND_CARTESIAN_CONTINIOUS = 4,
//    ENUM_COMMAND_JOINT_CONTINIOUS_LOOP = 5,
//    ENUM_COMMAND_CARTESIAN_CONTINIOUS_LOOP = 6,
//    ENUM_COMMAND_JOINT_TRAP = 7,
//    ENUM_COMMAND_SINGLE_AXIS_TRAP = 8,
//    ENUM_COMMAND_SINGLE_AXIS_SPLINE = 9,
//    ENUM_COMMAND_JOINT_SPLINE = 10,

//};


int MotionInstructionLogic::processMotionInstruction(int programNum, int startLine)
{
    qDebug()<<"processMotionInstruction motion line="<<programMcController->mcControllerInterpreter->getRunningNumber();;
    programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_BEGIN);

    //平滑运动指令段时，首条加入队列，其余条根据上下文判断是否已经加入过。
    //单条运动指令时，只加入一条进队列，不做预处理

    //判断以前是否加入过该条指令
    if(lastAddedLineId>=startLine)
    {
        for(int i = startLine; i >0;i--)
        {
            //圆弧第二条判断
            if(MOTION == (*programSet)[programNum][i-1].type && 0==(*programSet)[programNum][i-1].motion.cnt
                    && (*programSet)[programNum][i-1].motion.motionFormat==E_MOTION_C
                    && (*programSet)[programNum][i-1].motion.rate<0.01)
            {
                 programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_CIRCILE_END_POINT_BEFORE);
                 qDebug()<<"圆弧第二行，已加过轨迹";
                return 0;
            }

            //cnt平滑判断
            if(MOTION == (*programSet)[programNum][i-1].type && 0!=(*programSet)[programNum][i-1].motion.cnt)
            {
                programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_CNT_POINT_BEFORE);
                qDebug()<<"平滑行号，已加过轨迹";
                return 0;
            }
            else if(MOTION == (*programSet)[programNum][i-1].type && 0==(*programSet)[programNum][i-1].motion.cnt)
            {
                programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_NEW_POINT_BEGIN);

                break;
            }

        }

    }

    //加入指令，非平滑加一条，平滑指令加一完整段.圆弧指令至少加２条。
    MoveParameter tmpMovePara;
    QVector <MoveParameter> tmpMotionParaList;
    PalletPoint tmpPalletPoint;
    tmpPalletPoint.rowColumnLayer.resize(3);
    int circleInstructionCount=0;
    for(int j = startLine; j < (*programSet)[programNum].count();j++)
    {
        if(MOTION == (*programSet)[programNum][j].type)
        {
            switch((*programSet)[programNum][j].motion.motionFormat)
            {
            case E_MOTION_J:
            {
                tmpMovePara.moveType = ENUM_COMMAND_JOINT_VARIABLE;
                break;
            }
            case E_MOTION_L:
            {
                tmpMovePara.moveType = ENUM_COMMAND_LINE;
                break;
            }
            case E_MOTION_C:
            {
                tmpMovePara.moveType = ENUM_COMMAND_CIRCLE;
                break;
            }
            case E_MOTION_C2:
            {
                break;
            }
            default:
            {
                qDebug()<<"unknow moveType"<<(*programSet)[programNum][j].motion.motionFormat;
            break;
            }
            }

            tmpMovePara.velocityUnit = (*programSet)[programNum][j].motion.rateFormat;
            tmpMovePara.velocity = (float)(*programSet)[programNum][j].motion.rate/100;
            tmpMovePara.evFlag = (*programSet)[programNum][j].motion.evFlag;
            tmpMovePara.evVelocity = (*programSet)[programNum][j].motion.evValue;
            tmpMovePara.wjnt = (*programSet)[programNum][j].motion.motionWjntType;
            tmpMovePara.pathFlag = (*programSet)[programNum][j].motion.motionPthType;
            tmpMovePara.incFlag = (*programSet)[programNum][j].motion.motionIncType;
            //提取目标点
            if(1!=getInsTargetPoint((*programSet)[programNum][j].motion,
                                    tmpMovePara.pointType,tmpMovePara.index,tmpMovePara.targetPoint))
            {
                programEngine->cmdError(programNum);
                return -1;
            }

            tmpMovePara.cntRatio = (*programSet)[programNum][j].motion.cnt;
            tmpMovePara.acc = (*programSet)[programNum][j].motion.acc;
            tmpMovePara.lineNumber = j;
            //圆弧指令处理，使至少加２条
            if(tmpMovePara.moveType==ENUM_COMMAND_CIRCLE )
            {
               circleInstructionCount+=1;
               programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_CIRCILE_FIRST_POINT);

            }
            tmpMotionParaList.append(tmpMovePara);
            if(0==tmpMovePara.cntRatio && tmpMovePara.moveType!=ENUM_COMMAND_CIRCLE)
            {
                programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_FINE_POINT_END);
                break;
            }
            if(0==tmpMovePara.cntRatio && tmpMovePara.moveType==ENUM_COMMAND_CIRCLE && circleInstructionCount==2)
            {
                programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_CIRCILE_END_POINT);
                break;
            }
        }
    }


    if( tmpMotionParaList.size() )
    {
        programEngine->setMotionInstructionAdded(tmpMotionParaList.back().lineNumber);//   最后加入的行号
        programMcController->mcControllerInterpreter->addTrajectoryCommand(tmpMotionParaList);
        //debug
        for(int i=0;i<tmpMotionParaList.size();i++)
        {
            lastAddedLineId=tmpMotionParaList[i].lineNumber;
            qDebug()<<"add motion line "<<tmpMotionParaList[i].lineNumber<<", moveType"<<tmpMotionParaList[i].moveType
                      <<", cnt"<<tmpMotionParaList[i].cntRatio
                     <<", targetPoint"<<tmpMotionParaList[i].targetPoint.positionValue.size();
        }
    }
    else
    {
        programEngine->setMotionInstructionAdded(0);
        qDebug()<<"未添加运动命令setMotionInstructionAdded(0)";
    }
    return 1;
}

int MotionInstructionLogic::processMotionInstruction_carvingTp(int programNum, int startLine)
{
    qDebug()<<"processMotionInstruction_carvingTp motion line="<<programMcController->mcControllerInterpreter->getRunningNumber();;
    programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_BEGIN);

    //平滑运动指令段时，首条加入队列，其余条根据上下文判断是否已经加入过。
    //单条运动指令时，只加入一条进队列，不做预处理

    //判断以前是否加入过该条指令
    if( (lastAddedLineId-startLine)>=maxLineCatshDepth_carvingTp)
    {
       qDebug()<<"已加过轨迹cnt line"<<startLine;
       return 0;

    }
    else if( lastAddedLineId==startLine && 0==(*programSet)[programNum][startLine].motion.cnt)
    {
       qDebug()<<"已加过轨迹fine line"<<startLine;
       return 0;

    }

    if(lastAddedLineId>=startLine && 0==(*programSet)[programNum][lastAddedLineId].motion.cnt)
    {
        qDebug()<<"等待平滑段尾巴结束"<<startLine;
        return 0;
    }


    //加入指令，非平滑加一条，平滑指令加一完整段.圆弧指令至少加２条。
    MoveParameter tmpMovePara;
    QVector <MoveParameter> tmpMotionParaList;
    PalletPoint tmpPalletPoint;
    tmpPalletPoint.rowColumnLayer.resize(3);
    int circleInstructionCount=0;
    for(int j = max(lastAddedLineId+1,startLine); j < (*programSet)[programNum].count();j++)
    {
        if(false == (*programSet)[programNum][j].isParsed)
        {
            qDebug()<<"行还未提取数据机构，line "<< j;
            programEngine->cmdError(programNum);
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3310, robotId ,j);
            return -1;
        }
        if(MOTION == (*programSet)[programNum][j].type)
        {
            switch((*programSet)[programNum][j].motion.motionFormat)
            {
            case E_MOTION_J:
            {
                tmpMovePara.moveType = ENUM_COMMAND_JOINT_VARIABLE;
                //j时不支持摆焊
                E_WAVE_STATUS tmpwaveStatus=programMcController->mcControllerInterpreter->getWaveMotionStatus();
                if(E_WAVE_STATUS_STOPED!=tmpwaveStatus)
                {
                    programMcController->mcControllerInterpreter->stopWave();
                    programEngine->cmdError(programNum);
                    addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3312, robotId ,j+1);
                    return -1;
                }
                break;
            }
            case E_MOTION_L:
            {
                tmpMovePara.moveType = ENUM_COMMAND_LINE;
                break;
            }
            case E_MOTION_C:
            {
                tmpMovePara.moveType = ENUM_COMMAND_CIRCLE;
                break;
            }
            case E_MOTION_C2:
            {
                break;
            }
            default:
            {
                qDebug()<<"unknow moveType"<<(*programSet)[programNum][j].motion.motionFormat;
            break;
            }
            }

            tmpMovePara.velocityUnit = (*programSet)[programNum][j].motion.rateFormat;
            tmpMovePara.velocity = (float)(*programSet)[programNum][j].motion.rate/100;
            tmpMovePara.evFlag = (*programSet)[programNum][j].motion.evFlag;
            tmpMovePara.evVelocity = (*programSet)[programNum][j].motion.evValue;
            tmpMovePara.wjnt = (*programSet)[programNum][j].motion.motionWjntType;
            tmpMovePara.pathFlag = (*programSet)[programNum][j].motion.motionPthType;
            tmpMovePara.incFlag = (*programSet)[programNum][j].motion.motionIncType;
            //提取目标点
            if(1!=getInsTargetPoint((*programSet)[programNum][j].motion,
                                    tmpMovePara.pointType,tmpMovePara.index,tmpMovePara.targetPoint))
            {
                programEngine->cmdError(programNum);
                return -1;
            }

            tmpMovePara.cntRatio = (*programSet)[programNum][j].motion.cnt;
            tmpMovePara.acc = (*programSet)[programNum][j].motion.acc;
            tmpMovePara.lineNumber = j;
            //圆弧指令处理，使至少加２条
            if(tmpMovePara.moveType==ENUM_COMMAND_CIRCLE )
            {
               circleInstructionCount+=1;
               programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_CIRCILE_FIRST_POINT);

            }
            tmpMotionParaList.append(tmpMovePara);
            if(0==tmpMovePara.cntRatio && tmpMovePara.moveType!=ENUM_COMMAND_CIRCLE)
            {
                programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_FINE_POINT_END);
                break;
            }
            if(0==tmpMovePara.cntRatio && tmpMovePara.moveType==ENUM_COMMAND_CIRCLE && circleInstructionCount==2)
            {
                programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_CIRCILE_END_POINT);
                break;
            }
            if(tmpMotionParaList.size()>=maxLineCatshDepth_carvingTp)
            {
                qDebug()<<"预处理条数达到设定"<<maxLineCatshDepth_carvingTp;
                programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_FINE_POINT_END);
                break;
            }

        }
    }


    if( tmpMotionParaList.size() )
    {
        //debug
        for(int i=0;i<tmpMotionParaList.size();i++)
        {

            qDebug()<<"emcTP add motion line "<<tmpMotionParaList[i].lineNumber<<", moveType"<<tmpMotionParaList[i].moveType
                      <<", cnt"<<tmpMotionParaList[i].cntRatio
                     <<", targetPoint"<<tmpMotionParaList[i].targetPoint.positionValue.size();
        }


        int tmpKey=0;
        int tmpMoveType=tmpMotionParaList[0].moveType;
        if(1==tmpMotionParaList.size() &&
                (ENUM_COMMAND_JOINT_VARIABLE==tmpMoveType||ENUM_COMMAND_SINGLE_AXIS_TRAP==tmpMoveType
                 || ENUM_COMMAND_SINGLE_AXIS_SPLINE==tmpMoveType||ENUM_COMMAND_JOINT_SPLINE==tmpMoveType))
        {
            if(startLine!=tmpMotionParaList[0].lineNumber)
            {
                addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3309, robotId ,tmpMotionParaList[0].lineNumber+1);
                qDebug()<<"error3309a,";
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }

            tmpKey= programMcController->mcControllerInterpreter->addTrajectoryCommand(tmpMotionParaList);

        }
//        else if(2==tmpMotionParaList.size() &&
//                ENUM_COMMAND_CIRCLE==tmpMoveType)
//        {
//            if(startLine!=tmpMotionParaList[0].lineNumber)
//            {
//                addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3311, robotId ,
//                        tmpMotionParaList[0].lineNumber+1);
//                return E_PROGRAM_LOOP_RESULT_ERROR;
//            }
//            //coordinate规划器的开始位置没有同步了。
//            tmpKey= programMcController->mcControllerInterpreter->addTrajectoryCommand(tmpMotionParaList);

//        }
        else
        {
            //判断是否掺杂关节
            for(int i=0;i<tmpMotionParaList.size();i++)
            {
                tmpMoveType=tmpMotionParaList[i].moveType;
                if((ENUM_COMMAND_JOINT_VARIABLE==tmpMoveType||ENUM_COMMAND_SINGLE_AXIS_TRAP==tmpMoveType
                         || ENUM_COMMAND_SINGLE_AXIS_SPLINE==tmpMoveType||ENUM_COMMAND_JOINT_SPLINE==tmpMoveType))
                {
                    addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3309, robotId ,tmpMotionParaList[i].lineNumber+1);
                    qDebug()<<"error3309b,";
                    return E_PROGRAM_LOOP_RESULT_ERROR;
                }
            }
            tmpKey= programMcController->mcControllerInterpreter->transCommandQueueToCarvingModule(tmpMotionParaList);

        }

        if(1==tmpKey)
        {
            lastAddedLineId=tmpMotionParaList.back().lineNumber;
            programEngine->setMotionInstructionAdded(tmpMotionParaList.back().lineNumber);//   最后加入的行号
        }
        else
        {
            qDebug()<<"add line error,line "<<startLine;
        }
        return tmpKey;

    }
    else
    {
        programEngine->setMotionInstructionAdded(0);
        qDebug()<<"未添加运动命令setMotionInstructionAdded(0),line "<<startLine;
    }
    return 1;
}

int MotionInstructionLogic::processMotionInstruction_gcode(int programNum, int startLine)
{
    qDebug()<<"processMotionInstruction_gcode motion line="<<programMcController->mcControllerInterpreter->getRunningNumber();;
    programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_BEGIN);

    //平滑运动指令段时，首条加入队列，其余条根据上下文判断是否已经加入过。
    //单条运动指令时，只加入一条进队列，不做预处理

    //判断以前是否加入过该条指令
    if( (lastAddedLineId-startLine)>=maxLineCatshDepth_carvingTp)
    {
       qDebug()<<"已加过轨迹cnt line"<<startLine;
       return 0;

    }
    else if( lastAddedLineId==startLine && 0==(*programSet)[programNum][startLine].motion.cnt)
    {
        //判断当前行是否要精确停止。
       qDebug()<<"已加过轨迹fine line"<<startLine;
       return 0;

    }

    if(lastAddedLineId>=startLine && 0==(*programSet)[programNum][lastAddedLineId].motion.cnt)
    {
        qDebug()<<"等待平滑段尾巴结束"<<startLine;
        return 0;
    }


    //加入指令，非平滑加一条，平滑指令加一完整段.圆弧指令至少加２条。
    MoveParameter tmpMovePara;
    QVector <MoveParameter> tmpMotionParaList;
    PalletPoint tmpPalletPoint;
    tmpPalletPoint.rowColumnLayer.resize(3);
    int circleInstructionCount=0;
    for(int j = max(lastAddedLineId+1,startLine); j < (*programSet)[programNum].count();j++)
    {
        if(false == (*programSet)[programNum][j].isParsed)
        {
            qDebug()<<"行还未提取数据机构，line "<< j;
            programEngine->cmdError(programNum);
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3310, robotId ,j);
            return -1;
        }
        if(MOTION == (*programSet)[programNum][j].type)
        {
            switch((*programSet)[programNum][j].motion.motionFormat)
            {
            case E_MOTION_J:
            {
                tmpMovePara.moveType = ENUM_COMMAND_JOINT_VARIABLE;
                //j时不支持摆焊
                E_WAVE_STATUS tmpwaveStatus=programMcController->mcControllerInterpreter->getWaveMotionStatus();
                if(E_WAVE_STATUS_STOPED!=tmpwaveStatus)
                {
                    programMcController->mcControllerInterpreter->stopWave();
                    programEngine->cmdError(programNum);
                    addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3312, robotId ,j+1);
                    return -1;
                }
                break;
            }
            case E_MOTION_L:
            {
                tmpMovePara.moveType = ENUM_COMMAND_LINE;
                break;
            }
            case E_MOTION_C:
            {
                tmpMovePara.moveType = ENUM_COMMAND_CIRCLE;
                break;
            }
            case E_MOTION_C2:
            {
                break;
            }
            default:
            {
                qDebug()<<"unknow moveType"<<(*programSet)[programNum][j].motion.motionFormat;
            break;
            }
            }

            tmpMovePara.velocityUnit = (*programSet)[programNum][j].motion.rateFormat;
            tmpMovePara.velocity = (float)(*programSet)[programNum][j].motion.rate/100;
            tmpMovePara.evFlag = (*programSet)[programNum][j].motion.evFlag;
            tmpMovePara.evVelocity = (*programSet)[programNum][j].motion.evValue;
            tmpMovePara.wjnt = (*programSet)[programNum][j].motion.motionWjntType;
            tmpMovePara.pathFlag = (*programSet)[programNum][j].motion.motionPthType;
            tmpMovePara.incFlag = (*programSet)[programNum][j].motion.motionIncType;
            //提取目标点
            if(1!=getInsTargetPoint((*programSet)[programNum][j].motion,
                                    tmpMovePara.pointType,tmpMovePara.index,tmpMovePara.targetPoint))
            {
                programEngine->cmdError(programNum);
                return -1;
            }

            tmpMovePara.cntRatio = (*programSet)[programNum][j].motion.cnt;
            tmpMovePara.acc = (*programSet)[programNum][j].motion.acc;
            tmpMovePara.lineNumber = j;
            //圆弧指令处理，使至少加２条
            if(tmpMovePara.moveType==ENUM_COMMAND_CIRCLE )
            {
               circleInstructionCount+=1;
               programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_CIRCILE_FIRST_POINT);

            }
            tmpMotionParaList.append(tmpMovePara);
            if(0==tmpMovePara.cntRatio && tmpMovePara.moveType!=ENUM_COMMAND_CIRCLE)
            {
                programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_FINE_POINT_END);
                break;
            }
            if(0==tmpMovePara.cntRatio && tmpMovePara.moveType==ENUM_COMMAND_CIRCLE && circleInstructionCount==2)
            {
                programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_CIRCILE_END_POINT);
                break;
            }
            if(tmpMotionParaList.size()>=maxLineCatshDepth_carvingTp)
            {
                qDebug()<<"预处理条数达到设定"<<maxLineCatshDepth_carvingTp;
                programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_FINE_POINT_END);
                break;
            }

        }
    }


    if( tmpMotionParaList.size() )
    {
        //debug
        for(int i=0;i<tmpMotionParaList.size();i++)
        {

            qDebug()<<"emcTP add motion line "<<tmpMotionParaList[i].lineNumber<<", moveType"<<tmpMotionParaList[i].moveType
                      <<", cnt"<<tmpMotionParaList[i].cntRatio
                     <<", targetPoint"<<tmpMotionParaList[i].targetPoint.positionValue.size();
        }


        int tmpKey=0;
        int tmpMoveType=tmpMotionParaList[0].moveType;
        if(1==tmpMotionParaList.size() &&
                (ENUM_COMMAND_JOINT_VARIABLE==tmpMoveType||ENUM_COMMAND_SINGLE_AXIS_TRAP==tmpMoveType
                 || ENUM_COMMAND_SINGLE_AXIS_SPLINE==tmpMoveType||ENUM_COMMAND_JOINT_SPLINE==tmpMoveType))
        {
            if(startLine!=tmpMotionParaList[0].lineNumber)
            {
                addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3309, robotId ,tmpMotionParaList[0].lineNumber+1);
                qDebug()<<"error3309a,";
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }

            tmpKey= programMcController->mcControllerInterpreter->addTrajectoryCommand(tmpMotionParaList);

        }
//        else if(2==tmpMotionParaList.size() &&
//                ENUM_COMMAND_CIRCLE==tmpMoveType)
//        {
//            if(startLine!=tmpMotionParaList[0].lineNumber)
//            {
//                addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3311, robotId ,
//                        tmpMotionParaList[0].lineNumber+1);
//                return E_PROGRAM_LOOP_RESULT_ERROR;
//            }
//            //coordinate规划器的开始位置没有同步了。
//            tmpKey= programMcController->mcControllerInterpreter->addTrajectoryCommand(tmpMotionParaList);

//        }
        else
        {
            //判断是否掺杂关节
            for(int i=0;i<tmpMotionParaList.size();i++)
            {
                tmpMoveType=tmpMotionParaList[i].moveType;
                if((ENUM_COMMAND_JOINT_VARIABLE==tmpMoveType||ENUM_COMMAND_SINGLE_AXIS_TRAP==tmpMoveType
                         || ENUM_COMMAND_SINGLE_AXIS_SPLINE==tmpMoveType||ENUM_COMMAND_JOINT_SPLINE==tmpMoveType))
                {
                    addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3309, robotId ,tmpMotionParaList[i].lineNumber+1);
                    qDebug()<<"error3309b,";
                    return E_PROGRAM_LOOP_RESULT_ERROR;
                }
            }
            tmpKey= programMcController->mcControllerInterpreter->transCommandQueueToCarvingModule(tmpMotionParaList);

        }

        if(1==tmpKey)
        {
            lastAddedLineId=tmpMotionParaList.back().lineNumber;
            programEngine->setMotionInstructionAdded(tmpMotionParaList.back().lineNumber);//   最后加入的行号
        }
        else
        {
            qDebug()<<"add line error,line "<<startLine;
        }
        return tmpKey;

    }
    else
    {
        programEngine->setMotionInstructionAdded(0);
        qDebug()<<"未添加运动命令setMotionInstructionAdded(0),line "<<startLine;
    }
    return 1;
}

int MotionInstructionLogic::processAxisMoveInstruction(int startLine, const AxisMoveInstruction &motionInstruction)
{
    programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_BEGIN);

    QVector<double> jointsPosition, jointsVel,jointAcc, jointTorque;
    programMcController->mcControllerInterpreter->getJoint_Feedback( jointsPosition, jointsVel,jointAcc, jointTorque);
    if(motionInstruction.axisId>jointsPosition.size() || motionInstruction.axisId<=0)
    {
        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3305, robotId ,motionInstruction.axisId);
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }

    MoveParameter tmpMovePara;
    QVector <MoveParameter> tmpMotionParaList;
    tmpMovePara.moveType = ENUM_COMMAND_SINGLE_AXIS_TRAP;

    tmpMovePara.velocityUnit = motionInstruction.rateUnitTypeFlag;
    tmpMovePara.velocity = (float)motionInstruction.velocityRatio/100.0;
    tmpMovePara.evFlag = 0;
    tmpMovePara.evVelocity = 0;
    tmpMovePara.wjnt = 0;
    tmpMovePara.pathFlag = 0;
    tmpMovePara.incFlag = 0;
    tmpMovePara.pointType = ENUM_NORMAL_POINT;
    tmpMovePara.index = 0;//固定点号
    tmpMovePara.targetPoint.jointFlag=0;//关节点
    tmpMovePara.targetPoint.positionValue.resize(motionInstruction.axisId);

    double axisTargetPos;
    switch (motionInstruction.positionReference) {
    case E_POSITION_REFERENCE_ABSOLUTE:
        axisTargetPos=motionInstruction.targetPosition;
        break;
    case E_POSITION_REFERENCE_INC:
        axisTargetPos=jointsPosition[motionInstruction.axisId-1]+motionInstruction.targetPosition;
        break;
    case E_POSITION_REFERENCE_MAP:
    {
        //根据地图角度，发绝对命令，运动到绝对位置
        PositionInfo positionReturn;
        programMcController->mcControllerInterpreter->getVehiclePosition_specified(positionReturn,E_VEHICLE_POSITION_TYPE_ROS);
        getBestPositionFromAngle(motionInstruction.targetPosition,positionReturn.position.rotate*180.0/M_PI,
                                 jointsPosition[motionInstruction.axisId-1] ,axisTargetPos);
        break;
    }
    case E_POSITION_REFERENCE_FOLLOW:
    {
        //根据地图角度，发绝对命令，运动到绝对位置
        CoupleConfigParameter coupleParaOut;
        if(1!=programMcController->mcControllerInterpreter->getCoupleConfigInfo(
                                        motionInstruction.axisId-1, coupleParaOut))
        {
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3307, robotId ,motionInstruction.axisId);
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        if(coupleParaOut.driveAxisId>=jointsPosition.size())
        {
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3306, robotId ,coupleParaOut.driveAxisId+1);
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        axisTargetPos=jointsPosition[coupleParaOut.driveAxisId]+motionInstruction.targetPosition;
        qDebug()<<"driveAxis Position"<<jointsPosition[coupleParaOut.driveAxisId]
                  <<"move"<<motionInstruction.targetPosition
                    <<"axisTargetPos"<<axisTargetPos;
        break;
    }

    default:
        break;
    }

    tmpMovePara.targetPoint.positionValue[motionInstruction.axisId-1]=axisTargetPos;
    qDebug()<<"final axisTargetPos"<<axisTargetPos;
    tmpMovePara.cntRatio = 0;
    tmpMovePara.acc = -1;
    tmpMovePara.lineNumber = startLine;
    tmpMovePara.singleAxisFlag=motionInstruction.axisId-1;

    tmpMotionParaList.append(tmpMovePara);

    programEngine->setMotionInstructionAdded(tmpMotionParaList.size());
    programMcController->mcControllerInterpreter->addTrajectoryCommand(tmpMotionParaList);

    return 1;
}

int MotionInstructionLogic::reset()
{
    lastAddedLineId=-1;
//    waitFlag=false;
    return 1;
}


int MotionInstructionLogic::getSetMotionBlock(int startLineNum)
{
    //bug todo
    return 1;


}


int MotionInstructionLogic::pretreatMotion()
{

    QVector <QVector <int> > allInterruptList;
    allInterruptList.clear();

    allInterruptList.resize(programSet->size());
    for(int programNum = 0; programNum < programSet->size();programNum++)
    {
        QVector <int> interruptList;
        interruptList.clear();
        interruptList.append(0);

        for(int i = 0; i < (*programSet)[programNum].count();i++)
        {
            if((WAIT == (*programSet)[programNum][i].type)
                    ||(GOTO == (*programSet)[programNum][i].type)
                    ||(JUMP == (*programSet)[programNum][i].type)
                    ||(LABEL == (*programSet)[programNum][i].type)
                    ||(SELECT == (*programSet)[programNum][i].type)
                    ||(IF == (*programSet)[programNum][i].type)
                    ||(CALL == (*programSet)[programNum][i].type)
                    ||(FOR == (*programSet)[programNum][i].type)
                    ||(ENDFOR == (*programSet)[programNum][i].type)
                    ||(MOTION_SKIP == (*programSet)[programNum][i].type)
                    ||(COORDINATE == (*programSet)[programNum][i].type)
                    ||(GRIPPER == (*programSet)[programNum][i].type)
                    ||(INS_PALLET == (*programSet)[programNum][i].type))
            {
                interruptList.append(i);
//                qDebug() << "==i="<<i<<"\n\n\n\n\n";
            }
            else
            {
                if(i == (*programSet)[programNum].count() - 1)
                {
                    if(interruptList[interruptList.size() - 1] == i)
                    {

                    }
                    else
                    {
                        interruptList.append(i);
                    }
                }
            }
        }

        allInterruptList[programNum] = interruptList;
    }


     for(int programNum = 0; programNum < programSet->size();programNum++)
     {
         QVector <int> interruptList;
         interruptList.resize(allInterruptList[programNum].size());
         interruptList = allInterruptList[programNum];

        int k = 0;
        while(k < interruptList.size() - 1)
        {
            QVector <int> tmpMotionList;
            tmpMotionList.clear();
            for(int i = interruptList[k];i <= interruptList[k + 1];i++)
            {
                //            if(ENUM_COMMAND_RESET == engineStatus)
                //            {
                //                return 1;
                //            }
                if(MOTION == (*programSet)[programNum][i].type)
                {
                    tmpMotionList.append(i);
//                    qDebug() << "*****i****"<<i<<"\n\n\n\n\n\n";
                }
            }
            if(tmpMotionList.size())
            {
                (*programSet)[programNum].motionMap.insert(tmpMotionList[0],tmpMotionList.size());
            }
            k++;
        }
    }

    //    for(int i = 0;i < (*programSet)[programNum].motionMap.size();i++)
    //    {
    //        qDebug() << "=i="<<i<<"==pc=="<<(*programSet)[programNum].motionMap.keys().at(i)
    //                 <<"=len=="<<(*programSet)[programNum].motionMap.values().at(i)<<endl;
    //    }
     return 0;
}

int MotionInstructionLogic::setLaserTrackConfigParameter(LaserTrackConfig paraIn)
{

    return writeConfigFile(configPath,paraIn);
}

int MotionInstructionLogic::getLaserTrackConfigParameter(struct LaserTrackConfig &paraOut)
{
    paraOut=laserTrackConfig;
    return 1;
}

E_PROGRAM_LOOP_RESULT MotionInstructionLogic::laserCameraTrackLine(int programNum, int startLine)
{
    filterX->resetAll();
    filterY->resetAll();
    filterZ->resetAll();
    PointPro tcpPosition;
    E_PROGRAM_LOOP_RESULT tmpKey=programEngine->fetchWeldPoint(tcpPosition,D_FETCH_WAIT_COUNT);
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=tmpKey)
    {
        qDebug()<<"error,programEngine->fetchWeldPoint";
        return tmpKey;
    }

    if(ENUM_COMMAND_LINE!=(*programSet)[programNum][startLine].motion.motionFormat)
    {
        qDebug()<<"error,不支持的跟踪轨迹类型";
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }
    int lineId=startLine;
    //加入第一个点
    tmpKey=addOnePointToMove(programNum,startLine,tcpPosition,lineId);
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=tmpKey)
    {
        qDebug()<<"error,加入第一个点";
        //恢复行号
        programEngine->setRunningNumber_robotArmControl(startLine);
        return tmpKey;
    }

    int p_num = (*programSet)[programNum][startLine].motion.p - 1;
    if(E_POSITION_P == (*programSet)[programNum][startLine].motion.positionFormat)
    {

        if( p_num<getPointProList->size() && p_num >= 0)
        {

        }
        else
        {
            qDebug()<<"test common point,无此点位数据："<< p_num;
            programEngine->cmdError(programNum);
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3007, robotId ,p_num+1);
            //恢复行号
            programEngine->setRunningNumber_robotArmControl(startLine);
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
    }
    else
    {
        qDebug()<<"error,不支持1111";
        //恢复行号
        programEngine->setRunningNumber_robotArmControl(startLine);
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }

    //结束距离计算
    PointPro startPos,finishPos;
    programMcController->mcControllerInterpreter->getCurrentCoordinatePoint_Feedback(startPos);
    finishPos=programMcController->mcControllerInterpreter->pointCoordinateToCartesian((*getPointProList)[ p_num ]);
    double targetLength=sqrt(pow(startPos.positionValue[0]-finishPos.positionValue[0],2)+
            pow(startPos.positionValue[1]-finishPos.positionValue[1],2)+
            pow(startPos.positionValue[2]-finishPos.positionValue[2],2));

    //加入第xxx点
    PointPro oldPosition=tcpPosition;
    PointPro middlePosition;
    double currentLength;
    double deltaMove;
    filterX->arithmeticMeanfilter(tcpPosition.positionValue[0],laserTrackConfig.filterCount);
    filterY->arithmeticMeanfilter(tcpPosition.positionValue[1],laserTrackConfig.filterCount);
    filterZ->arithmeticMeanfilter(tcpPosition.positionValue[2],laserTrackConfig.filterCount);
    do
    {
//        qDebug()<<"currentLength"<<currentLength<<"targetLength"<<targetLength<<"deltaMove"<<deltaMove;
        usleep(D_FETCH_SLEEP);
        if(ENUM_COMMAND_RESET == programEngine->getEngineStatue())
        {
            qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();
            //恢复行号
            programEngine->setRunningNumber_robotArmControl(startLine);
           return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
        }
        else if(ENUM_COMMAND_CEASE == programEngine->getEngineStatue())
        {
            if(programEngine->getQuickStopFlag())
            {//报错后，轨迹被清空．重新运行该条命令
                if(1!=isMotionInstructionFinished(lineId))//一定要加行有没有完成判断
                {
                    //reset() 使下次在运行该行时，才能重新添加轨迹。
                    //恢复行号
                    programEngine->setRunningNumber_robotArmControl(startLine);
                    return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
                }

            }
        }

        E_PROGRAM_LOOP_RESULT tmpFetchKey=programEngine->fetchWeldPoint(middlePosition,D_FETCH_WAIT_COUNT);
        if(E_PROGRAM_LOOP_RESULT_NORMAL!=tmpFetchKey)
        {
            //恢复行号
            programEngine->setRunningNumber_robotArmControl(startLine);
            qDebug()<<"error,programEngine->fetchWeldPoint";
            return tmpFetchKey;
        }

        currentLength=sqrt(pow(startPos.positionValue[0]-middlePosition.positionValue[0],2)+
                pow(startPos.positionValue[1]-middlePosition.positionValue[1],2)+
                pow(startPos.positionValue[2]-middlePosition.positionValue[2],2));
        deltaMove=sqrt(pow(oldPosition.positionValue[0]-middlePosition.positionValue[0],2)+
                pow(oldPosition.positionValue[1]-middlePosition.positionValue[1],2)+
                pow(oldPosition.positionValue[2]-middlePosition.positionValue[2],2));


        if(deltaMove<D_TRACK_MIN_DISTANCE)
        {
            continue;
        }
        oldPosition=middlePosition;
        lineId++;
        middlePosition.positionValue[0]=filterX->arithmeticMeanfilter(middlePosition.positionValue[0],laserTrackConfig.filterCount);
        middlePosition.positionValue[1]=filterY->arithmeticMeanfilter(middlePosition.positionValue[1],laserTrackConfig.filterCount);
        middlePosition.positionValue[2]=filterZ->arithmeticMeanfilter(middlePosition.positionValue[2],laserTrackConfig.filterCount);
        tmpKey=addOnePointToMove(programNum,startLine,middlePosition,lineId);
        if(E_PROGRAM_LOOP_RESULT_NORMAL!=tmpKey)
        {
            //恢复行号
            programEngine->setRunningNumber_robotArmControl(startLine);
            qDebug()<<"error,加入第x点";
            return tmpKey;
        }
    }while(currentLength<targetLength);



    //等待行号
    E_PROGRAM_LOOP_RESULT tmpRsult=waitMotionAccomplish(lineId);
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=tmpRsult)
    {
        return tmpRsult;
    }
    //恢复行号
    programEngine->setRunningNumber_robotArmControl(startLine);
    programEngine->calculateNextLineToRun(programNum);
    return programEngine->isTheLastInstruction(programNum);
}

E_PROGRAM_LOOP_RESULT MotionInstructionLogic::addOnePointToMove(int programNum, int startLine,
                                                                PointPro targetPointIn,int lineIdIn)
{
    MoveParameter tmpMovePara;
    QVector <MoveParameter> tmpMotionParaList;
    tmpMovePara.moveType = ENUM_COMMAND_LINE;
    tmpMovePara.pointType = ENUM_NORMAL_POINT;
    tmpMovePara.velocityUnit = (*programSet)[programNum][startLine].motion.rateFormat;
    tmpMovePara.velocity = (float)(*programSet)[programNum][startLine].motion.rate/100;
    tmpMovePara.evFlag = (*programSet)[programNum][startLine].motion.evFlag;
    tmpMovePara.evVelocity = (*programSet)[programNum][startLine].motion.evValue;
    tmpMovePara.wjnt = (*programSet)[programNum][startLine].motion.motionWjntType;
    tmpMovePara.pathFlag = (*programSet)[programNum][startLine].motion.motionPthType;
    tmpMovePara.incFlag = (*programSet)[programNum][startLine].motion.motionIncType;

    tmpMovePara.targetPoint=targetPointIn;
    tmpMovePara.cntRatio = 50;
    tmpMovePara.acc = (*programSet)[programNum][startLine].motion.acc;
    tmpMovePara.lineNumber = lineIdIn;
    tmpMotionParaList.append(tmpMovePara);

    int tmpaddKey= programMcController->mcControllerInterpreter->transCommandQueueToCarvingModule(tmpMotionParaList);
    if(1==tmpaddKey)
    {
        lastAddedLineId=tmpMotionParaList.back().lineNumber;
        programEngine->setMotionInstructionAdded(tmpMotionParaList.back().lineNumber);//   最后加入的行号
    }
    else
    {
        qDebug()<<"add line error,line "<<startLine;
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }
    return E_PROGRAM_LOOP_RESULT_NORMAL;
}

bool MotionInstructionLogic::isFleetMonitorOk()
{
    diMuTex.lock();

    if(0==fleetMonitor_diIndex.size())
    {
        diMuTex.unlock();
        return true;
    }
    bool tmpReturn;
    if(fleetMonitor_isOr)
    {
        tmpReturn=true;
        for(int i=0;i<fleetMonitor_diIndex.size();i++)
        {
            if(fleetMonitor_diValue[i]==ioController->getDi(fleetMonitor_diIndex[i]))
            {
                tmpReturn=false;
            }
        }
    }
    else
    {
        tmpReturn=false;
        for(int i=0;i<fleetMonitor_diIndex.size();i++)
        {
            if(fleetMonitor_diValue[i]!=ioController->getDi(fleetMonitor_diIndex[i]))
            {
                tmpReturn=true;
            }
        }
    }

    diMuTex.unlock();
    return tmpReturn;
}

bool MotionInstructionLogic::isConditionTrig(int diIndex, bool diValue)
{
    if(diValue==ioController->getDi(diIndex))
    {
        return true;
    }
    return false;
}


int MotionInstructionLogic::isMotionInstructionFinished(int waitLine)
{
    float lineStatus;
    float floatLineDifference;
    lineStatus=programMcController->mcControllerInterpreter->getRunningNumber();
    floatLineDifference=lineStatus-waitLine;
//    qDebug()<<"lineStatus="<<lineStatus<<",waitLine="<<waitLine<<",floatLineDifference="<<floatLineDifference;
    //cnt????
//    if(floatLineDifference > 0.8 && floatLineDifference < 0.99)//?????fine类型指令，肯定是运行完一条才会加下一行。
    if(floatLineDifference > 0.8 )
    {
        qDebug()<<"lineStatus"<<lineStatus<<"waitLine"<<waitLine<<"floatLineDifference"<<floatLineDifference;
        return 1;
    }

    return -1;
}

int MotionInstructionLogic::getInsTargetPoint(const MotionInstruction &motion,
                                              int &pointTypeOut,int &pointIdOut,PointPro &targetPointout)
{

    //堆跺点
    if((E_POSITION_PAL_A == motion.positionFormat)
            ||(E_POSITION_PAL_R == motion.positionFormat)
            ||(E_POSITION_PAL_BTM == motion.positionFormat))
    {
        pointTypeOut = ENUM_PALLET_POINT;
        int palletNumber = motion.palNum -1;
        int pValue = motion.p - 1;
        if(INDIRECT == motion.pointType)
        {
            pValue = (int)programSet->regRList[pValue] - 1;
        }
        pointIdOut = pValue;

        std::vector<struct PalletMiddleInfo> palletinfoOut;
        programEngine->getPalletConditionInfo(palletinfoOut);
        if(palletNumber<0 || palletNumber>=palletinfoOut.size())
        {
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3313, robotId ,palletNumber+1);
            return -1;
        }
        if(E_POSITION_PAL_A == motion.positionFormat)
        {
            if(pValue<0 || pValue>=palletinfoOut[palletNumber].pointAnPointPro.size())
            {
                addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3314, robotId ,pValue+1);
                return -1;
            }
            targetPointout=palletinfoOut[palletNumber].pointAnPointPro[pValue];
        }
        else if(E_POSITION_PAL_BTM == motion.positionFormat)
        {
            targetPointout=palletinfoOut[palletNumber].btmPointPro;
        }
        else if(E_POSITION_PAL_R == motion.positionFormat)
        {
            if(pValue<0 || pValue>=palletinfoOut[palletNumber].pointRnPointPro.size())
            {
                addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3315, robotId ,pValue+1);
                return -1;
            }
            targetPointout=palletinfoOut[palletNumber].pointRnPointPro[pValue];
        }
        else
        {
            qDebug()<<"errork ,unknow positionFormat"<<motion.positionFormat;
            return -200;
        }
        return 1;

    }//P或PR点
    else if((E_POSITION_P == motion.positionFormat)
            ||(E_POSITION_PR == motion.positionFormat))
    {
        int p_num = motion.p - 1;
        if(INDIRECT == motion.pointType)
        {
            p_num = (int)programSet->regRList[p_num] - 1;
        }
        pointIdOut = p_num;
        if(E_POSITION_P == motion.positionFormat)
        {
            pointTypeOut = ENUM_NORMAL_POINT;
            if( p_num<getPointProList->size() && p_num >= 0)
            {
                targetPointout = (*getPointProList)[ p_num ];
            }
            else
            {
                qDebug()<<"test common point,无此点位数据："<< p_num;
                addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3007, robotId ,p_num+1);
                return -1;
            }
        }
        else if(E_POSITION_PR == motion.positionFormat)
        {
            pointTypeOut = ENUM_PR_POINT;
            if( p_num < programSet->regPrList.size() && p_num >= 0)
            {
                //不要检查工具坐标号和用户坐标号。
                programSet->regPrList[ p_num ].configInt.clear();
                programSet->regPrList[ p_num ].configString.clear();
                programSet->regPrList[ p_num ].ufValue=programMcController->mcControllerInterpreter->getUserIndex();
                programSet->regPrList[ p_num ].utValue=programMcController->mcControllerInterpreter->getToolIndex();
                targetPointout = programSet->regPrList[ p_num ];
                qDebug() <<"targetPoint:PR[p_num]"<<p_num<<"uf"<<programSet->regPrList[ p_num ].ufValue
                        <<"ut"<<programSet->regPrList[ p_num ].utValue;
                //regData
                for(int k = 0; k < targetPointout.positionValue.size();k++)
                {
                    qDebug() <<targetPointout.positionValue[k];
                }
            }
            else
            {
                qDebug()<<"***=====无此点位数据："<< p_num;
                addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3301, robotId,p_num+1 );
                return -1;
            }
        }

        //世界位置偏移处理
        MOTION_OFFSET_TYPE motionOffsetType = motion.motionOffsetType;
        MOTION_TOOL_OFFSET_TYPE motionToolOffsetType = motion.motionToolOffsetType;
        if((MOTION_OFFSET_PR == motionOffsetType)
                ||(MOTION_OFFSET_PR_R == motionOffsetType))
        {

            int tmpPRindex_offset;
            if(MOTION_OFFSET_PR == motionOffsetType)
            {
                tmpPRindex_offset = motion.offsetPrIndex - 1;
            }
            if(MOTION_OFFSET_PR_R == motionOffsetType)
            {
                int tmpRIndex= motion.motionOffsetValue - 1;
                if(tmpRIndex < programSet->regRList.size() && tmpRIndex >= 0)
                {
                    tmpPRindex_offset = programSet->regRList[tmpRIndex];
                }
                else
                {
                    qDebug()<<"无此点位数据："<< p_num;
                    addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3017, robotId );
                    return -1;
                }
            }

            if((tmpPRindex_offset < programSet->regPrList.size())&&(tmpPRindex_offset >= 0))
            {

                    if(programSet->regPrList[tmpPRindex_offset].positionValue.size() <= targetPointout.positionValue.size())
                    {
                        for(int axis = 0; axis < programSet->regPrList[tmpPRindex_offset].positionValue.size();axis++)
                        {
                            targetPointout.positionValue[axis] +=  programSet->regPrList[tmpPRindex_offset].positionValue[axis];
                            qDebug()<<"运动补偿　targetPointout.positionValue[axis]"<<targetPointout.positionValue[axis]
                                      <<"PR补偿"<<programSet->regPrList[tmpPRindex_offset].positionValue[axis];
                        }
                    }
                    else
                    {
                        qDebug()<<"error：PR数据长度超过轴数"<< p_num;
                        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3012, robotId, programSet->regPrList[tmpPRindex_offset].positionValue.size());
                        return -1;
                    }

            }
            else
            {
                    qDebug()<<"error:PR索引超出范围"<< p_num;
                    addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3304, robotId ,tmpPRindex_offset+1);
                    return -1;
             }
        }

        //工具位置偏移处理　暂时不支持
        if((MOTION_TOOL_OFFSET_PR == motionToolOffsetType)
                ||(MOTION_TOOL_OFFSET_PR_R == motionToolOffsetType))
        {
            int ufNum1 = targetPointout.ufValue;
            int utNum1 = targetPointout.utValue;
            int jointFlag1 = targetPointout.jointFlag;

            int rValue = motion.motionToolOffsetType - 1;

            if(MOTION_TOOL_OFFSET_PR_R == motionToolOffsetType)
            {
                if(rValue < programSet->regRList.size() && rValue >= 0)
                {
                    rValue = programSet->regRList[rValue];
                }
                else
                {
                    qDebug()<<"无此点位数据："<< p_num;
                    addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3017, robotId );
                    return -1;
                }
            }

            if(rValue < programSet->regRList.size() && rValue >= 0)
            {
                int ufNum2 = programSet->regPrList[rValue].ufValue;
                int utNum2 = programSet->regPrList[rValue].utValue;
                int jointFlag2 =programSet->regPrList[rValue].jointFlag;

                if((ufNum1 == ufNum2)&&(utNum1 == utNum2))
                {
                    if(jointFlag1 == jointFlag2)
                    {
                        for(int axis = 0; axis < (int)targetPointout.positionValue.size();axis++)
                        {
//                                    tmpMovePara.targetPoint.positionValue[axis] +=  programSet->regPrList[rValue].positionValue[axis];
                        }
                    }
                    else
                    {
                        qDebug()<<"无此点位数据："<< p_num;
                        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3012, robotId );
                        return -1;
                    }
                }
            }
            else
            {
                    qDebug()<<"无此点位数据："<< p_num;
                    addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3302, robotId );
                    return -1;
             }
        }
    }
    return 1;
}

E_PROGRAM_LOOP_RESULT MotionInstructionLogic::addProgramMoveCommand_fleet(const QVector<int> &lineList,
                                                                   const QVector<RobotArmMoveParameter> &pathListParaIn)
{
    if(0==pathListParaIn.size() || lineList.size() !=pathListParaIn.size())
    {
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }


    //加入指令，非平滑加一条，平滑指令加一完整段.圆弧指令至少加２条。
    MoveParameter tmpMovePara;
    QVector <MoveParameter> tmpMotionParaList;

    for(int j = 0; j < pathListParaIn.size();j++)
    {

            switch(pathListParaIn[j].moveType)
            {
            case E_MOTION_J:
            {
                tmpMovePara.moveType = ENUM_COMMAND_JOINT_VARIABLE;
                break;
            }
            case E_MOTION_L:
            {
                tmpMovePara.moveType = ENUM_COMMAND_LINE;
                break;
            }
            case E_MOTION_C:
            {
                tmpMovePara.moveType = ENUM_COMMAND_CIRCLE;
                break;
            }
            default:
            {
                qDebug()<<"error, unknow moveType"<<pathListParaIn[j].moveType;
                return E_PROGRAM_LOOP_RESULT_ERROR;
            break;
            }
            }

            tmpMovePara.velocityUnit = E_RATE_PERCENT;
            tmpMovePara.velocity = (float)pathListParaIn[j].velocityRatio/100.0;
            tmpMovePara.evFlag = 0;
            tmpMovePara.evVelocity = 0;
            tmpMovePara.wjnt = 0;
            tmpMovePara.pathFlag = 0;
            tmpMovePara.incFlag = 0;


            tmpMovePara.pointType = ENUM_DIRECT_POINT;

            //不要检查工具坐标号和用户坐标号。
            tmpMovePara.targetPoint=pathListParaIn[j].targetPoint;
            tmpMovePara.targetPoint.configInt.clear();
            tmpMovePara.targetPoint.configString.clear();
            tmpMovePara.targetPoint.ufValue=programMcController->mcControllerInterpreter->getUserIndex();
            tmpMovePara.targetPoint.utValue=programMcController->mcControllerInterpreter->getToolIndex();

            tmpMovePara.cntRatio = 0;
            tmpMovePara.acc = -1;
            tmpMovePara.lineNumber = lineList[j];

            tmpMotionParaList.append(tmpMovePara);

    }


    if( tmpMotionParaList.size() )
    {
        programEngine->setMotionInstructionAdded(tmpMotionParaList.size());
        if(0==armTpType)
        {
            programMcController->mcControllerInterpreter->addTrajectoryCommand(tmpMotionParaList);
        }
        else
        {
            programMcController->mcControllerInterpreter->transCommandQueueToCarvingModule(tmpMotionParaList);
        }

    }
    else
    {
        programEngine->setMotionInstructionAdded(0);
    }

    return E_PROGRAM_LOOP_RESULT_NORMAL;
}

E_PROGRAM_LOOP_RESULT MotionInstructionLogic::waitMotionAccomplish_fleet(const QVector<int> &lineList)
{
    for(int i=0;i<lineList.size();i++)
    {
        E_PROGRAM_LOOP_RESULT resultKey=waitMotionAccomplish_fleet(lineList[i]);
        if(E_PROGRAM_LOOP_RESULT_NORMAL!=resultKey)
        {
            return resultKey;
        }
    }
    return E_PROGRAM_LOOP_RESULT_NORMAL;
}

E_PROGRAM_LOOP_RESULT MotionInstructionLogic::waitMotionAccomplish_fleet(int lineIndex)
{
    //完成判断，没有则阻塞等待
    int isFinished=-1;

    while(-1==isFinished)
    {
        if(false==isFleetMonitorOk())
        {
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3308, robotId, lineIndex);
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }

//        waitFlag=true;
        if(1==isMotionInstructionFinished(lineIndex))
        {
            isFinished=1;
        }

        if(-1==isFinished)
        {
            usleep(D_MOTION_INSTRUCTION_WAIT_SLEEP);
        }

        if(ENUM_COMMAND_RESET == programEngine->getEngineStatue())
        {

            qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();

           return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
        }

    }

    return  E_PROGRAM_LOOP_RESULT_NORMAL;
}

bool MotionInstructionLogic::isNeedAddTraj(int programNum, int startLine)
{

}

int MotionInstructionLogic::getBestPositionFromAngle(double mapTargetAngle, double agvRotation_current,
                                                     double followAxisStartAngle,double &followAxisTargetOut)
{

    //可以正反转到达目标姿态，持正反转。
    double followAxisPos_map=agvRotation_current+followAxisStartAngle;
    translatePN180(followAxisPos_map);

    double rotate1=angleRotationJudgement(mapTargetAngle/180*M_PI,followAxisPos_map/180*M_PI);

    followAxisTargetOut=followAxisStartAngle+rotate1/M_PI*180;

    return 1;
}

int MotionInstructionLogic::translatePN180(double &degree)
{
    int turns=degree/360;
    degree=degree-turns*360;
    if(degree>180.0)
    {
        degree-=360;
    }
    else if(degree<-180.0)
    {
        degree+=360;
    }
    else
    {
    }
    return 1;
}

double MotionInstructionLogic::angleRotationJudgement(double targetAngle, double initialAngle)
{
    double angle1,angle2,angle3;
    angle1=targetAngle-initialAngle;
    angle2=angle1+2*M_PI;
    angle3=angle1-2*M_PI;
    double minNum;
    if(fabs(angle1)<=fabs(angle2))
    {
        minNum=angle1;
    }
    else
    {
        minNum=angle2;
    }

    if(fabs(angle3)<=fabs(minNum))
    {
        minNum=angle3;
    }

    return minNum;

}

int MotionInstructionLogic::writeConfigFile(QString configFileName, LaserTrackConfig laserTrackConfigIn)
{

    qDebug() << "LaserCameraTrajServer::writeConfigFile ==== 初始化配置文件：" << configFileName;

    DomParser domparser;
    bool ok = domparser.openXml(configFileName, QIODevice::ReadWrite);
    if( !ok )
    {

        qDebug() << " 配置文件打开失败：" << configFileName;
        return -1;
    }


    QDomNode maindomNode = domparser.findSubNode(domparser.getRootDomElement(), "laserTrackConfig", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }


    QDomNode domNode = domparser.findSubNode(maindomNode, "filterCount", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.writeXml(domNode, laserTrackConfigIn.filterCount);



    domparser.closeXml(true);
    laserTrackConfig=laserTrackConfigIn;
    return 1;
}

int MotionInstructionLogic::initialConfigFile(QString configFileName)
{
    qDebug() << "LaserCameraTrajServer::initialConfigFile ==== 初始化配置文件：" << configFileName;

    DomParser domparser;
    bool ok = domparser.openXml(configFileName, QIODevice::ReadOnly);
    if( !ok )
    {

        qDebug() << " 配置文件打开失败：" << configFileName;
        return -1;
    }

    QDomNode maindomNode = domparser.findSubNode(domparser.getRootDomElement(), "laserTrackConfig", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }


    QDomNode domNode = domparser.findSubNode(maindomNode, "filterCount", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(domNode, laserTrackConfig.filterCount);

    domparser.closeXml();
    return 1;
}



E_PROGRAM_LOOP_RESULT MotionInstructionLogic::waitMotionAccomplish(int lineIndexIn,
                                        bool isMonitorDi,int diIndex, bool diValue)
{
    int isFinished=-1;
    while(-1==isFinished)
    {
//        waitFlag=true;
        isFinished=isMotionInstructionFinished(lineIndexIn);
        if(-1==isFinished)
        {
            usleep(D_MOTION_INSTRUCTION_WAIT_SLEEP);
        }

        if(ENUM_COMMAND_RESET == programEngine->getEngineStatue())
        {
            qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();

           return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
        }
        else if(ENUM_COMMAND_CEASE == programEngine->getEngineStatue())
        {
//            if(1==programMcController->mcControllerInterpreter->isTrajectoryEmpty())//报错后，轨迹被清空．重新运行该条命令
            if(programEngine->getQuickStopFlag())
            {//报错后，轨迹被清空．重新运行该条命令
                if(1!=isMotionInstructionFinished(lineIndexIn))//一定要加行有没有完成判断
                {
                    //reset() 使下次在运行该行时，才能重新添加轨迹。
                    return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
                }

            }
        }

//        programEngine->weldWorkingMonitor();//焊接检测．

        //skip监视
        if(isMonitorDi)
        {
            if(isConditionTrig(diIndex,diValue))
            {
                programMcController->mcControllerInterpreter->allStop();
                while(1!=programMcController->mcControllerInterpreter->isTrajectoryEmpty())
                {
                    usleep(D_MOTION_INSTRUCTION_WAIT_SLEEP);
                }
                programMcController->mcControllerInterpreter->allStartRun();
                return E_PROGRAM_LOOP_RESULT_JUMP;
            }
        }

    }
//    waitFlag=false;
    qDebug()<<"waitMotionAccomplish finished,,,line"<<lineIndexIn;
    return E_PROGRAM_LOOP_RESULT_NORMAL;
}



void MotionInstructionLogic::addMsg(int messageLevel, string componentName, string messageType,
                           int messageCode, int robotId,
                           int parameter1, int parameter2, int parameter3, int parameter4,QString message)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;
   // tmpMsg.time = t1.tv_sec;


    QString infomationStr;

    switch(messageCode)
    {
    case 3316:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",laserCameraConfig.xml读取失败！").arg(parameter1)+message;
    }
        break;
    case 3315:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(";码跺逃离点号[%1]超出有效范围！").arg(parameter1)+message;
    }
        break;
    case 3314:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(";码跺接近点号[%1]超出有效范围！").arg(parameter1)+message;
    }
        break;
    case 3313:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(";码跺号[%1]超出有效范围！").arg(parameter1)+message;
    }
        break;
    case 3312:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(";指令行[%1]错误，J关节运动不支持摆焊，先终止摆焊！").arg(parameter1)+message;
    }
        break;
    case 3311:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(";  指令行[%1]错误，连续插补内部不能包含C运动指令！").arg(parameter1)+message;
    }
        break;
    case 3310:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(";  行[%1]还没有提取数据结构！").arg(parameter1)+message;
    }
        break;
    case 3309:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(";  指令行[%1]错误，连续插补内部不能包含Ｊ运动指令！").arg(parameter1)+message;
    }
        break;
    case 3308:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(";  运动监视DI时，符合监视条件！").arg(parameter1)+message;
    }
        break;
    case 3307:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(";  单轴运动轴,耦合随动轴[%1]未配置！").arg(parameter1)+message;
    }
        break;
    case 3306:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(";  单轴运动轴,耦合主动轴[%1]索引超出范围！").arg(parameter1)+message;
    }
        break;
    case 3305:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(";  单轴运动轴[%1]索引超出范围！").arg(parameter1)+message;
    }
        break;
    case 3304:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr("; 运动指令的补偿PR[%1]索引超出范围！").arg(parameter1)+message;
    }
        break;
    case 3303:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr("; 耦合控制模块没有轴[%1]！").arg(parameter1)+message;
    }
        break;
    case 3302:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr("; 运动指令Offset的PR[%1]点找不到数据内容！").arg(parameter1)+message;
    }
        break;
    case 3301:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr("; 运动指令的PR[%1]点找不到数据内容！").arg(parameter1)+message;
    }
        break;

    case 3005:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr("; %1,第%2行， IO号超出已有逻辑ＩＯ范围，请检查指令或ＩＯ配置，").arg(message).arg(parameter3)
                +QString::number(parameter1) + ">="
        +QString::number(parameter2);
        break;


    }
    case 3007:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr("; 运动指令的P[%1]点找不到数据内容！").arg(parameter1)+message;
    }
        break;
    case 3012:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",运动指令的PR[%1]补偿数据的个数超过范围").arg(parameter1)+message;

    }
        break;
    case 3017:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",寄存器指令错误，寄存器的范围超过限定的最大范围，请修改")+message;
    }
        break;
    case 3023:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("第") +
               QString::number(parameter1+1) +
               QObject::tr("行运行出现异常（算法模块进入停止状态）！")+message;

        break;
    }




    default:
    {

    }
    }


    tmpMsg.MessageInformation = infomationStr.toStdString();

    // qDebug()<<"53444444444444444444444"<< tmpMsg.messageCode;
    MessageLog::getInstance()->addMessage(tmpMsg);
}
